/*
 * SL_SingleSLAM.h
 *
 *  Created on: 2011-1-12
 *      Author: Danping Zou
 */

#ifndef SL_SINGLESLAM_H_
#define SL_SINGLESLAM_H_
#include "tracking/GPUKLT.h"
#include "tools/SL_VideoReader.h"
#include "math/SL_Matrix.h"

#include "slam/SL_Camera.h"
#include "slam/SL_FeaturePoints.h"
#include "slam/SL_PtrVec.h"
#include "slam/SL_NCCBlock.h"
#include "slam/SL_MapPointList.h"
#include "slam/SL_KeyPoseList.h"

#include "imgproc/SL_Image.h"
#include "tracking/SL_Track2D.h"
#include <string>

class SingleSLAMParam {
public:
	//divide the image plane into nRowBlk x nColBlk blocks
	//For each block, only one feature points is selected for pose estimation  
	int nRowBlk;
	int nColBlk;
	SingleSLAMParam() {
		//640x360
//		nRowBlk = 9;
//		nColBlk = 16;
		//640x480
		nRowBlk = 12;
		nColBlk = 16;
	}
};
class SingleSLAM: public SingleSLAMParam {
public:
	int camId;
	int W, H; //image size
	int blkW, blkH; //block size

	int m_nMappedStaticPts;

	VideoReader* videoReader;
	std::string videoFilePath;
	std::string calFilePath;
	int startFrameInVideo;
    int nInitFrm;

	GPUKLT m_tracker; //feature point tracker;
	Mat_d K, iK; //intrinsic matrix and its inverse
	Mat_d k_c, k_ud; //distortion and its inverse

	ImgRGB m_rgb; //color image
	ImgG m_img; //original image
	ImgG m_smallImg; //small image
	double m_smallScale;
public:
	FeaturePoints m_featPts;
	CamPoseList m_camPos;

	/* camera poses at key frames generated by self camera motion*/
	//KeyPoseList m_selfKeyPoseList;
	vector<KeyPose*> m_selfKeyPose;

	/* camera poses at key frames generated by self camera motion, motion of other cameras, few static points in one camera */
	KeyPoseList m_keyPose;

	KeyPose* m_prevKeyPos;
	KeyPose* m_lastKeyPos;
public:
	SingleSLAM();
	~SingleSLAM();
public:
	/* get current frame*/
	int currentFrame() {
		return m_tracker.currentFrame();
	}
	void propagateFeatureStates();
public:
	int getStaticMappedTrackNodes(std::vector<Track2DNode*>& nodes);
	int getUnMappedTrackNodes(std::vector<Track2DNode*>& nodes, int minLen);
	int getUnMappedAndDynamicTrackNodes(std::vector<Track2DNode*>& nodes,
			int minLen);
	int getNumMappedFeatPts();
	int getNumMappedStaticPts();
	int getNumMultiMappedFeatPts();
public:
	int getTrackedFeatPts(std::vector<FeaturePoint*>& vecFeatPts,
			Mat_d* featPts = 0, int trackLen = 5);
	int getUnMappedAndTrackedFeatPts(std::vector<FeaturePoint*>& vecFeatPts,
			Mat_d* featPts = 0, int trackLen = 5);
	/*
	 * get unmapped and tracked corresponding feature points which are not marked as 'dynamic')
	 */
	int getUnMappedAndTrackedCorrespondence(int f1, int f2,
			std::vector<Track2DNode*>& nodes1,
			std::vector<Track2DNode*>& nodes2);
	//get the mapped feature points with the number of visible views between numVis1 and numVis2
	int getUnMappedFeatPts(std::vector<FeaturePoint*>& vecFeatPts,
			Mat_d* featPts = 0);
	int getMonoMappedFeatPts(std::vector<FeaturePoint*>& vecFeatPts,
			Mat_d* featPts = 0);
	int getAllMappedFeatPts(std::vector<FeaturePoint*>& vecFeatPts,
			Mat_d* featPts = 0);
	int getMappedFeatPts(int numVis1, int numVis2,
			std::vector<FeaturePoint*>& mappedFeatPts, Mat_d* featPts);
	int getMappedDynPts(std::vector<FeaturePoint*>& mappedDynPts);
public:
	int initTracker(int f, vector<FeaturePoint*>& existFeatPoints);
	int initTracker(int f);
	void readFirstFrame();
	void grabReadFrame();
	int trackFeaturePoints();
	void updateCamParamForFeatPts(const double* intrin, CamPoseItem* camPos);

	/**
	 * choose feature points for pose estimation
	 */
	int chooseStaticFeatPts(std::vector<FeaturePoint*>& featPts);
	int chooseDynamicFeatPts(std::vector<FeaturePoint*>& featPts);

	/**
	 * compute the camera pose using the chosen 3D- 2D point pairs
	 */
	int fastPoseUpdate3D();
	/**
	 * compute the current camera pose from the 3D - 2D point pairs
	 */
	int poseUpdate3D(bool largeErr);

	/**
	 * compute the camera pose using both the 3D-2D and 2D-2D correspondences
	 */
	int poseUpdate3D2D();

	/*
	 * detect unmapped dynamic points
	 * minLen : minimum length of 2D track
	 * outNum : minimum number of frames a feature point moving out of epipolar line
	 * maxEpiErr : threshold for determining whether a feature point has moved out of the epipolar line
	 */
	int detectDynamicFeaturePoints(int maxLen, int minLen, int outNum,
			double maxEpiErr);
	/*
	 * check whether slam will fail
	 * 0 : normal
	 * 1 : large decreasing speed of the number of static points
	 * 2 : static points gather in a small area
	 * 3 : few static points
	 */
	int willFail();
	/*
	 * get camera translation from the last self key frame (the last one in m_selfKeyFrmLst)
	 */
	double getCameraTranslationSelf();
	/**
	 * get the view angle change from the last self key frame ( the last one in m_selfKeyFrmLst)
	 */
	double getViewAngleChangeSelf(const double center[3]);
	/**
	 * add a key frame for this camera 
	 * bSelfMotion : true - caused by self translation; false - caused by other reasons
	 */
	KeyPose* addKeyPose(bool bSelfMotion);
	int getCurFeatPts(std::vector<FeaturePoint*>& pFeatPoints);
	/**
	 * triangulate new map points
	 */
	int newMapPoints(std::vector<MapPoint*>& mapPts, double maxEpiErr = 2.0,
			double maxNcc = 0.75);
	/* refine the map point*/
	void refineTriangulation(const FeaturePoint* fp, double M[3],
			double cov[9]);
	//registrate the map points to current feature points
	int regMapPoints(std::vector<MapPoint*>& mapPts, PtrVec<NCCBlock>& nccblks,
			double maxEpiErr = 3.0, double maxNcc = 0.7, double scale = 0.3);
	void removeFeatPts(int frame);
	void removeKeyPoseImgs(int frame);
	/* feed extra feature points to the tracker*/
	void feedExtraFeatPtsToTracker(std::vector<FeaturePoint*>& featPts);
public:
	//debug
	void saveCamPoses(const char* filePath, int startFrame = -1);
	void saveFeatureTracks(const char* filePath, int minLen = 5,
			int startFrame = -1);
};
FeaturePoint* searchNearestFeatPt(FeaturePoints& featPts, int f, double m[2],
		double maxDist);
FeaturePoint* searchMahaNearestFeatPt(FeaturePoints& featPts, int f,
		double m[2], double var[4], double maxDist);
void featPoint2Mat(std::vector<FeaturePoint*>& pFeatPoints, Mat_d& matPts);
#endif /* SL_SINGLESLAM_H_ */
